#include "dialogregistrationtargets.h"
#include "ui_dialogregistrationtargets.h"
#include<QDir>

dialogRegistrationTargets::dialogRegistrationTargets(QWidget *parent) :
    QDialog(parent),
    ui(new Ui::dialogRegistrationTargets)
{
    ui->setupUi(this);
    setWindowTitle("Set Registration Targets");

    curCam=0; //start with camera 0
    ui->comboBox_CameraNumber->addItem("0",0);
    ui->comboBox_CameraNumber->addItem("1",1);
    ui->comboBox_CameraNumber->setCurrentIndex(0);

    targetsReady=false;
    initDone=false;
    exiting=false;
    saveToProject=false;



}

dialogRegistrationTargets::~dialogRegistrationTargets()
{
    delete ui;
    exiting=1;
    cv::FileStorage fs;
    //write the targets to disk
    if(!targetCam0.empty() || !targetCam1.empty()) {
        fs.open(QString(dirTargets.path()+"/targets.yml").toStdString(), cv::FileStorage::WRITE);
        if(fs.isOpened()){
            fs<<"Cam0_Targets" << targetCam0;
            fs<<"Cam1_Targets" << targetCam1;
            fs<<"World_Targets" << targetWorld;
            fs.release();
        }
    }

    if(saveToProject) {
        if(fs.open((projName+"/targetsProject.yml").toStdString(),cv::FileStorage::READ)){
            //file exists.  Ask if we should overwrite
            QMessageBox msgBox;
            msgBox.setIcon(QMessageBox::Question);
            msgBox.setWindowTitle("Overwrite Targets In Project Folder");
            msgBox.setInformativeText("There is an existing project target File.  Overwrite?");
            msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No);
            msgBox.setDefaultButton(QMessageBox::No);
            int ret = msgBox.exec();
            if(ret==QMessageBox::Yes) {
                fs.release();
                if(fs.open((projName+"/targetsProject.yml").toStdString(),cv::FileStorage::WRITE))
                    fs<<"World_Targets" << targetWorld;


            }

        } else
            if(fs.open((projName+"/targetsProject.yml").toStdString(),cv::FileStorage::WRITE))
                fs<<"World_Targets" << targetWorld;
    } //end saveToProj

}

void dialogRegistrationTargets::init(QString event) {
    //make directory if not done already
    dirTargets.setPath(event + "/RegistrationTargets");
    if (!dirTargets.exists())
        dirTargets.mkpath(".");

    //read targets from disk, if they exist
    cv::FileStorage fs(QString(dirTargets.path()+"/targets.yml").toStdString(), cv::FileStorage::READ);
    if(fs.isOpened()){
        fs["Cam0_Targets"] >> targetCam0;
        fs["Cam1_Targets"] >> targetCam1;
        fs["World_Targets"]>> targetWorld;
        fs.release();
    }
    else {
        fs.open((projName+"/targetsProject.yml").toStdString(),cv::FileStorage::READ);
        if(fs.isOpened()){
            fs["World_Targets"]>> targetWorld;
            fs.release();
        }
    }
    targetStatus();
    initDone=true;
    on_pushButton_ReadImage_clicked();
}

void dialogRegistrationTargets::on_comboBox_CameraNumber_currentIndexChanged(int index)
{
    //Update Camera Selection
    curCam=index;
    if(initDone)
        on_pushButton_ReadImage_clicked();
}

void dialogRegistrationTargets::on_pushButton_GrabImage_clicked()
{
    //fire up camera and take a picture
    cv::VideoCapture capCam;   //camera capture


    // open camera
    capCam.open(curCam);

    std::string windowname="Grabbing Video Frame, Press Any Key to Grab, or Escape to Exit";
    cv::namedWindow(windowname);

    if(capCam.isOpened()){
        capCam.set(CV_CAP_PROP_FRAME_WIDTH, resWidth);      //Cam Width
        capCam.set(CV_CAP_PROP_FRAME_HEIGHT, resHeight);      //Cam Height

        for(;;){
            capCam >>matCap;
            cv::imshow(windowname, matCap);

            char c=cv::waitKey(1);
            if(c==27) { //check for 'Esc'
                break;
            } else if(c !=-1){ //any other key

                //save the image to the event/target folder
                QString tmp(dirTargets.path()+ "/Cam" + QString::number(curCam) + ".png");
                cv::imwrite(tmp.toStdString(),matCap);

                on_pushButton_ReadImage_clicked();  //calling this function to paint existing targets and update the UI.

//                //paste the image in to the dialog
//                cv::Mat tmpMat;
//                cv::cvtColor(matCap,tmpMat,CV_BGR2RGB);
//                QImage qimgCap((uchar*)tmpMat.data,tmpMat.cols,
//                               tmpMat.rows,tmpMat.step,QImage::Format_RGB888);
//                ui->label_TargetImage->setScaledContents(true);
//                ui->label_TargetImage->setPixmap(QPixmap::fromImage(qimgCap));

                break;

            }
        }
    }

    cv::destroyWindow(windowname); //close window

}

void dialogRegistrationTargets::on_pushButton_ReadImage_clicked()
{
    QString tmp(dirTargets.path()+ "/Cam" + QString::number(curCam) + ".png");
    matCap=cv::imread(tmp.toStdString());
    if(!matCap.empty()) {

        //draw the current targets on the image
        cv::Mat_<cv::Point2f> curCamMat;
        if(curCam==0) curCamMat=targetCam0; else curCamMat=targetCam1;

        for(int i = 0; i < curCamMat.rows; i++ ){
            cv::Point2i tmpUV=curCamMat(i,0);
            cv::circle( matCap, tmpUV, 1, cv::Scalar(51,255,255), -1);
            std::stringstream ssX,ssY,ssZ;
            cv::Point3f tmpXYZ=targetWorld(i,0);
            ssX << "X: "<<QString::number(tmpXYZ.x).toStdString();
            ssY << "Y: "<<QString::number(tmpXYZ.y).toStdString();
            ssZ << "Z: "<<QString::number(tmpXYZ.z).toStdString();
            //putX
            cv::putText(matCap,ssX.str(),cv::Point(tmpUV.x+5,tmpUV.y-10),
                    cv::FONT_HERSHEY_PLAIN, 0.8, cv::Scalar(51,255,255), 1, CV_AA);
            //putY
            cv::putText(matCap,ssY.str(),cv::Point(tmpUV.x+5,tmpUV.y),
                    cv::FONT_HERSHEY_PLAIN, 0.8, cv::Scalar(51,255,255), 1, CV_AA);
            //putZ
            cv::putText(matCap,ssZ.str(),cv::Point(tmpUV.x+5,tmpUV.y+10),
                    cv::FONT_HERSHEY_PLAIN, 0.8, cv::Scalar(51,255,255), 1, CV_AA);


        }





    //paste the image in to the dialog
        cv::Mat tmpMat;
        cv::cvtColor(matCap,tmpMat,CV_BGR2RGB);
        QImage qimgCap((uchar*)tmpMat.data,tmpMat.cols,
                       tmpMat.rows,tmpMat.step,QImage::Format_RGB888);
    ui->label_TargetImage->setScaledContents(true);
    ui->label_TargetImage->setPixmap(QPixmap::fromImage(qimgCap));
    }

}

void dialogRegistrationTargets::on_pushButton_SelectTargets_clicked()
{


    //get an original copy of the target image
    matTarget=cv::imread(QString(dirTargets.path()+ "/Cam" + QString::number(curCam) + ".png").toStdString());
    cv::cvtColor( matTarget, mat_gray, CV_BGR2GRAY );

    //get the image that already has targets drawn on it.
    if(!matCap.empty())
        mat1=matCap;  //soft copy
    else
        mat1=matTarget.clone(); //otherwise just get a duplicate of the original

    targetWinName="Use Mouse to Select a Region.  Press Escape to Finish";
    cvNamedWindow(targetWinName.c_str(), 1); //create window to show it
    cvSetMouseCallback(targetWinName.c_str(), dummy_mouseHandler, this); //connect mouse interaction to the image
    cv::imshow(targetWinName,mat1); //show the target image
    drag=0; //set drag to "not"
    for(;;) {
        if(cvWaitKey(100)==27) {  //If escape is pressed
            cvDestroyWindow(targetWinName.c_str());
            break;
        }
        if(!cvGetWindowHandle(targetWinName.c_str())) break;  //if the window gets closed (only works on Win32), Linux workaround below
                                                              // on Linux if Window is closed this function still keeps returning a value
    }
    if(exiting) return; // workaround for Linux.  Dialog is shutting down, but we were stuck in the loop above.


    targetStatus();
    on_pushButton_ReadImage_clicked(); //update the image
    return;
}

void dialogRegistrationTargets::mouseHandler(int event, int x, int y) {
    //used to handle selection of targets on the image

    // left button down
    if (event == CV_EVENT_LBUTTONDOWN && !drag) {
        point = cvPoint(x, y);
        drag  = 1;
    }

    // dragging the mouse to select a region
    if (event == CV_EVENT_MOUSEMOVE && drag) {

        cv::Mat tmp=mat1.clone();
        cv::rectangle(tmp,point,cvPoint(x,y),CV_RGB(255, 0, 0),1);
        cv::imshow(targetWinName,tmp);
    }

    // released left button
    if (event == CV_EVENT_LBUTTONUP && drag) {
        int winsize=4; //search window size for algorithm
        int minSize=2*winsize+5; //region selection must be at least twice + 5

        if(abs(x-point.x)>minSize && abs(y-point.y)>minSize) { //make sure selected area is sufficient

            cv::Rect r(point.x<x?point.x:x,
                       point.y<y?point.y:y,
                       point.x<x?x - point.x:point.x-x,
                       point.y<y?y - point.y:point.y-y);
            cv::Mat smallImg = mat_gray(r);
            cv::Mat mat1_roi=mat1(r);
            std::vector<cv::Point2f> corners;
            cv::goodFeaturesToTrack(smallImg, corners, 1, 0.01, 10);

            cv::Size winSize = cv::Size( winsize, winsize );
            cv::Size zeroZone = cv::Size( -1, -1 );
            cv::TermCriteria criteria = cv::TermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 40, 0.001 );

            /// Calculate the refined corner locations
            cv::cornerSubPix( smallImg, corners, winSize, zeroZone, criteria );

            /// Write them down
            for( unsigned int i = 0; i < corners.size(); i++ )
            { std::cout<<" -- Refined Corner ["<<i<<"]  ("<<corners[i].x+point.x<<","<<corners[i].y+point.y<<")"<<std::endl; }

            // Draw circle on them
            for(unsigned int i = 0; i < corners.size(); i++ )
            { cv::circle( mat1_roi, corners[i], 1, cv::Scalar(0,0,255), -1); }
            cv::imshow(targetWinName,mat1);
            drag = 0;

            dialogTargetCoordinates TargCoord;
            TargCoord.setUV(corners[0].x+point.x,corners[0].y+point.y);

            //if world points are already known then load them as default values in the dialog
            int worldRows=targetWorld.rows;
            int cam0rows=targetCam0.rows;
            int cam1rows=targetCam1.rows;
            if(curCam==0 && worldRows>cam0rows)
                TargCoord.setXYZ(targetWorld(cam0rows,0));
            else if(curCam==1 && worldRows>cam1rows)
                TargCoord.setXYZ(targetWorld(cam1rows,0));

            if(TargCoord.exec() == QDialog::Accepted){ // add it to the target list


                if(curCam==0) {
                    targetCam0.push_back(cv::Point2f(point.x+corners[0].x,point.y+corners[0].y) );
                    if(targetCam0.rows>targetWorld.rows)
                        targetWorld.push_back(cv::Point3f( TargCoord.WorldX,TargCoord.WorldY,TargCoord.WorldZ));
                }
                else {
                    targetCam1.push_back(cv::Point2f(point.x+corners[0].x,point.y+corners[0].y));
                    if(targetCam1.rows>targetWorld.rows)
                        targetWorld.push_back(cv::Point3f( TargCoord.WorldX,TargCoord.WorldY,TargCoord.WorldZ));
                }


                //debug - ouput values to console
                std::cout<<"X: "<<TargCoord.WorldX
                        <<" Y: "<<TargCoord.WorldY
                       <<" Z: "<<TargCoord.WorldZ
                      <<std::endl;

                // putText(Mat& img, const string& text, Point org, int fontFace, double fontScale, Scalar color, int thickness=1, int lineType=8, bool bottomLeftOrigin=false )
                //std::string x=QString::number(TargCoord.WorldX).toStdString();
                std::stringstream ssX,ssY,ssZ;
                ssX << "X: "<<QString::number(TargCoord.WorldX).toStdString();
                ssY << "Y: "<<QString::number(TargCoord.WorldY).toStdString();
                ssZ << "Z: "<<QString::number(TargCoord.WorldZ).toStdString();
                //putX
                cv::putText(mat1,ssX.str(),cv::Point(corners[0].x+point.x+5,corners[0].y+point.y-10),
                        cv::FONT_HERSHEY_PLAIN, 0.8, cvScalar(0,0,255), 1, CV_AA);
                //putY
                cv::putText(mat1,ssY.str(),cv::Point(corners[0].x+point.x+5,corners[0].y+point.y),
                        cv::FONT_HERSHEY_PLAIN, 0.8, cvScalar(0,0,255), 1, CV_AA);
                //putZ
                cv::putText(mat1,ssZ.str(),cv::Point(corners[0].x+point.x+5,corners[0].y+point.y+10),
                        cv::FONT_HERSHEY_PLAIN, 0.8, cvScalar(0,0,255), 1, CV_AA);
                cv::imshow(targetWinName,mat1);

                targetStatus();
            }

        }
        else { //cancel dragging and repaint
            cv::imshow(targetWinName,mat1);
            drag = 0;
        }
    }

    /* user click right button: reset all */
    if (event == CV_EVENT_RBUTTONUP)
    {
        mat1=matTarget.clone();
        imshow(targetWinName,mat1);
        drag = 0;
    }
}





void dialogRegistrationTargets::on_pushButton_ClearTargets_clicked()
{

    QMessageBox msgBox;
    msgBox.setIcon(QMessageBox::Question);
     msgBox.setWindowTitle("Clear Targets");
     msgBox.setInformativeText("Erase the targets & Remove them from the event folder?");
     msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No);
     msgBox.setDefaultButton(QMessageBox::No);
     int ret = msgBox.exec();


     if(ret==QMessageBox::Yes)
    {
    //clear Camera Targets and erase their files
    targetCam0.release();
    targetCam1.release();
    QFile::remove( QString(dirTargets.path()+ "/targets.yml") );

    targetStatus();
    on_pushButton_ReadImage_clicked();
    }
}

void dialogRegistrationTargets::targetStatus()
{
    int min = 4;
    int cam0=targetCam0.rows;
    int cam1=targetCam1.rows;

    if(cam0>=min)
        ui->label_Cam0Status->setText("Cam0 Status:  COMPLETE");
    if(cam1>=min)
        ui->label_Cam1Status->setText("Cam1 Status:  COMPLETE");

    ui->label_Cam0Targets->setText("Targets Defined: "+QString::number(cam0));
    ui->label_Cam1Targets->setText("Targets Defined: "+QString::number(cam1));

    if(cam0>=min && cam1>=min) targetsReady=true;
    else targetsReady=false;
}

void dialogRegistrationTargets::on_checkBox_SaveToProject_clicked()
{
    saveToProject=!saveToProject;
}
